function [P, K, lambda, mu] = NMPC_get_ris_new (A, B, Bw)
%% Get the RIS 
% By Liang Hongpeng, hongpeng1214@foxmail.com

%% System description
% x' = A * x + g(x) + B * u + Bw * w
% g(x) = [0; -0.25 * x2^3];
% The designed local state-feeback control law is u = K * x.

%% Solve the optimization problem using YALMIP
% YALMIP提供了一个高层次的MATLAB接口，使用户能够轻松地定义和解决各种凸优化问题，包括线性规划、二次规划、半定规划等。
% 使用 YALMIP 工具箱定义了优化问题的决策变量和约束。
% 具体来说，使用了矩阵变量X和Y，以及标量变量μ来构建线性矩阵不等式（LMI）。

X_ = [3.94147,1.87342e-17,9.93964e-16;1.87342e-17,4.28458,3.01774e-17;9.93964e-16,3.01774e-17,4.7098];
Y_ = [0.0809691,-0.960407,-0.00156908;0.539556,0.0717489,-0.487559];
mu_ = [5.93334];

% Define decision matrix variables
[n,~] = size(A); % 3
[~,m] = size(B); % 2
yalmip('clear');
mu = sdpvar(1); % 标量，通过sdpvar定义，用于描述鲁棒性的界限
X = diag(sdpvar(n,1)); % 对角矩阵，通过sdpvar定义，表示鲁棒不变集的形状  diag(sdpvar(n,1))
Y = sdpvar(m,n,'full'); % 矩阵，通过sdpvar定义，用于计算控制器参数 ,'full'
lambda0 = 0.2; % 0.3给定的常数:越大，变化越小 origin:sdpvar(1) 
%lambda0 = sdpvar(1);
I = eye(3);

% 构建线性矩阵不等式 LMI约束，包括状态方程和鲁棒性项，以及添加相关约束，构建了一个优化问题。(论文Lemma 2.)
LMI = [(A * X + B * Y)' + A * X + B * Y + (-lambda0) * X, Bw; Bw', mu*I]; % origin
const = [LMI >= 0, X >= 0, mu >=0]; % origin
%obj = -logdet(X);% origin

%LMI = [(A * X + B * Y)' + A * X + B * Y + (-lambda0) * X + Bw *mu*I*Bw'];% new worse
%LMI = [(A * X + B * Y)' + A * X + B * Y + (-lambda0) * X + mu*I]; % new same

%LMI = [(A * X )' + A * X + (-lambda0) * X + mu*I]; % new same

% for mindopt 
% LMI     = [X * A']; % test
% LMI2    = [Y'* B'];
% LMI3    = [A * X];
% LMI4    = [B * Y];
% LMI5    = [(-lambda0*I) * X];
% LMI6    = [mu*I]; % mu*I  /  Bw'*mu*I*Bw
% const = [ LMI+LMI2+LMI3+LMI4+LMI5+LMI6 >= 0, X >= 0, mu >=0 ];% 所有项求和的迹大于等于0

obj = 0; % new -trace(X) / -X % 为了mosek求解问题可行，但是导致acados第150次迭代报错

%% Solving
% 明确指定优化目标和求解器
ops = sdpsettings('solver','Mosek', 'verbose', 0);
optimize(const,obj,ops); % 最小化obj

%% Get results
X1 = value(X); % 通过value函数获取最优解，分别代表鲁棒不变集的形状和控制器参数
Y1 = value(Y);
P = X1^(-1); % X1的逆矩阵，用于计算鲁棒不变集
K = Y1 * P; % 控制器参数
%lambda = value(lambda0);
lambda = lambda0; % 给定的常数
mu = value(mu); % 代表了鲁棒性的界限
end
